/*
 * @Date         2022-07-10 10:01:24
 * @LastEditors  Tennikl
 * @LastEditTime 2022-07-10 10:03:42
 * @FilePath     \D_SJK_2022\D_SJK_2022\USER\CODE\dirction\direction.c
 */
#include "direction.h"
#include "include.h"

#define CruisingSpeed BasicSpeed    //找灯速度
//#define CruisingDir 0
#define FindedSpeed (BasicSpeed+AddSpeed)     //踩灯速度



void dir_judge(uint8_t *Loc, int *speed, int* dir)
{
    
    if (*Loc == 0 && *(Loc + 1) == 0)
    {
        *dir = CruisingDir;
        *speed = CruisingSpeed;
    }
    else
    {
        *dir = (64>*(Loc)?-1:1)*sqrt((64>*(Loc)?64-*(Loc):*(Loc)-64)*(*(Loc+1)));
        *speed = FindedSpeed;
    }
    *dir = constrain_float((int)*dir , -Dirlimit, Dirlimit);
    
   
    // //测试控速效果
   
    // if((count_1s/10)%2 == 0)
    //     *speed = 50;
    // else
    //     *speed = 0;
    
}